/*
 * Copyright (C) 2006-2012 TongYan Corporation
 * All rights reserved.
 *
 * @brief info:
 *
 * @author: 王哲成 - wangzhecheng@yeah.net
 * @date: 2021.05.25
 * @last modified: 2021-05-25 16:16
 *
 */

#include "../include/GaussNewton.hpp"
#include <cassert>

bool GaussNewton::updateSearchPoint(const ParamSet &param_set,
                                    Candidate &candidate) const {
  // 高斯牛顿法只能解决最小二乘问题
  assert(m_ptrTarFunc->ifLeastSquareProblem());

  /* ISGLeastSquareProblem *ptrLSP = */
  /*     dynamic_cast<ISGLeastSquareProblem *>(m_ptrTarFunc); */
  // 计算fx的梯度矩阵Jx(负方向)
  Vector dk = m_ptrTarFunc->gradientVector(candidate.first, param_set.dx());
  if (dk.squaredNorm() < m_dEpsilon * m_dEpsilon)
    return true;
  // 计算海塞矩阵Hx
  Matrix Hx = m_ptrTarFunc->HessianMatrix(candidate.first, param_set.dx());

  // Armijo法计算步长
  double lambda = Armijo(candidate.first, param_set.dx(), dk);

  // dx = -H(x)^-1*J(x)
  candidate.first += lambda * Hx.colPivHouseholderQr().solve(dk);

  return false;
}
